﻿using System;
using System.Windows.Forms;
using System.Drawing;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using OpenGL;

namespace MathTest
{
	class MathGL : GLForm
	{
		public MathGL()
			: base()
		{
			Size = new System.Drawing.Size(800, 600);
			Location = new Point(0, 0);
		}

		RigidBody B = new RigidBody(1);
		GLText T;

		protected override void InitGL()
		{
			T = new GLText("Arial");
			//B.MissileWireframe(4, 0.5);
			B.BuildBuffer();
			//B.Position = V3D.Green;
			//B.orientation.Q *= B_Matrix.Rotation3D(V3D.Blue, 45);
		}

		public void DrawMesh(RigidBody b)
		{
			GL.glEnableClientState(GL.GL_VERTEX_ARRAY);
			GL.glVertexPointer3d(0, b.VertexBuffer);
			if (b.IndexBuffer != null)
				GL.glDrawElements(GL.GL_TRIANGLES, b.IndexBuffer.Length, GL.GL_UNSIGNED_INT, b.IndexBuffer);
			if (b.LineIndexBuffer != null)
				GL.glDrawElements(GL.GL_LINES, b.LineIndexBuffer.Length, GL.GL_UNSIGNED_INT, b.LineIndexBuffer);
			GL.glDisableClientState(GL.GL_VERTEX_ARRAY);
		}

		protected override void Process(double dt, double tt)
		{
			B.orientation.ClearForces();
			B.orientation.AddLocalForce(new V3D(0, -2, 0), new V3D(0, 10, 0));
			if (KeyPressed(Keys.Tab))
				B.orientation.AddLocalForce(V3D.Black, V3D.Red * 10);
			else if (KeyPressed(Keys.A))
			{
				B.orientation.LocalForces[0] = new Orientation3D.LocalForce(V3D.Green * -1, new V3D(2, 10, 0));
			}
			else if (KeyPressed(Keys.Y))
			{
				B.orientation.LocalForces[0] = new Orientation3D.LocalForce(V3D.Green * -1, new V3D(-2, 10, 0));
			}
			else
				B.orientation.LocalForces[0] = new Orientation3D.LocalForce(V3D.Green * -1, new V3D(0, 10, 0));
			B.orientation.ApplyGravity();
			double RT = 45;
			if (KeyPressed(Keys.Left))
			{
				B.orientation.AddLocalForce(V3D.Blue, V3D.Green * RT);
				B.orientation.AddLocalForce(-V3D.Blue, -V3D.Green * RT);
			}
			else if (KeyPressed(Keys.Right))
			{
				B.orientation.AddLocalForce(-V3D.Blue, V3D.Green * RT);
				B.orientation.AddLocalForce(V3D.Blue, -V3D.Green * RT);
			}
			if (KeyPressed(Keys.Up))
			{
				B.orientation.AddLocalForce(V3D.Red, -V3D.Green * RT);
				B.orientation.AddLocalForce(-V3D.Red, V3D.Green * RT);
			}
			else if (KeyPressed(Keys.Down))
			{
				B.orientation.AddLocalForce(V3D.Red, V3D.Green * RT);
				B.orientation.AddLocalForce(-V3D.Red, -V3D.Green * RT);
			}
			B.Process(dt);
		}

		protected override bool DrawGLScene()
		{
			GL.glClear(GL.GL_COLOR_BUFFER_BIT | GL.GL_DEPTH_BUFFER_BIT);
			GL.glLoadIdentity();
			MakeOrtho();
			GL.glCol3D(KeyPressed(Keys.A) ? V3D.Green : V3D.White * 0.5);
			GL.glBegin(GL.GL_TRIANGLES);
			GL.glVertex3d(0, 50, 0);
			GL.glVertex3d(-25, 0, 0);
			GL.glVertex3d(25, 0, 0);
			GL.glEnd();
			V3D V = new V3D(1, 1, -1);
			MakePerspective(60, 0.1f, 10000);
			GL.gluLookAt(5, 5, 5, 0, 0, 0, 0, 1, 0);
			GL.glBegin(GL.GL_LINES);
			GL.glCol3D(V3D.Red);
			GL.glV3D(new V3D(5, 0, 0));
			GL.glV3D(B.orientation.Q * V + B.orientation.Position);
			GL.glCol3D(V3D.Yellow);
			GL.glV3D(new V3D(0, 0, 5));
			GL.glV3D(V * B.orientation.Q + B.orientation.Position);
			GL.glEnd();
			//DrawMesh(B);
			GL.glPushMatrix();
			B_Matrix q = new B_Matrix(B.orientation.Q.Inverse());
			if (false)
			{
				B_Matrix q2 = new B_Matrix(B.orientation.Q);
				Console.WriteLine(q);
				Console.WriteLine(q2);
			}
			//q[3, 0] = B.Position.X;
			//q[3, 1] = B.Position.Y;
			//q[3, 2] = B.Position.Z;

			//double[] mdl = new double[16];
			//GL.glGetDoublev(GL.GL_MODELVIEW_MATRIX, mdl);
			//GL.glLoadIdentity();
			//GL.glTrans3D(B.Position);
			//GL.glMultMatrixd(mdl);
			GL.glMultMatrixd(q.Data);
			if (GL.glGetError() != 0)
			{
			}
			DrawMesh(B);
			GL.glCol3D(V3D.White);
			GL.DrawSign(1);
			GL.glCol3D(V3D.Green);
			GL.glBegin(GL.GL_LINES);
			GL.glV3D(V3D.Black);
			GL.glV3D(V);
			GL.glEnd();
			GL.glPopMatrix();

			MakeOrtho();
			GL.glCol3D(V3D.Blue);
			for (int i = 0; i < 4; i++)
				for (int j = 0; j < 4; j++)
					T.DrawString(B.orientation.Q[i, j].ToString("000.000"), j * 100 - 200, -i * 20 + 200, 20, 20, StringAlignment.Center);
			GL.glCol3D(V3D.Blue);
			for (int i = 0; i < B.orientation.LocalForces.Count; i++)
				T.DrawString(B.orientation.LocalForces[i].Point.ToString(3, 3) + " " + B.orientation.LocalForces[i].Force.ToString(3, 3), -400, -i * 20 + 200, 10, 10, StringAlignment.Center);
			T.DrawString(B.orientation.Acc.ToString(3, 3), 0, 120, 15, 15, StringAlignment.Center);
			T.DrawString(B.orientation.Velocity.ToString(3, 3), 0, 100, 15, 15, StringAlignment.Center);
			T.DrawString(B.Position.ToString(3, 3), 0, 80, 15, 15, StringAlignment.Center);
			return true;
		}
	}
}